{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "58c84834",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "pybullet build time: May 20 2022 19:44:17\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "# 从阿凯机器人工具箱导入二指夹爪\n",
    "from kyle_robot_toolbox.end_effector import Gripper2FUart"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9d05376",
   "metadata": {},
   "source": [
    "## 创建舵机对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "cbb18ff1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 创建夹爪对象\n",
    "gripper = Gripper2FUart(config_path=\"./config\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "8557d5df",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机卸力\n",
    "gripper.disable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "21377e02",
   "metadata": {},
   "source": [
    "## 采集样本1\n",
    "\n",
    "记录夹爪闭合时刻的舵机原始位置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "85b12e61",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "alpha_close=-0.4\n"
     ]
    }
   ],
   "source": [
    "alpha_close = gripper.get_servo_position()\n",
    "print(f\"alpha_close={alpha_close}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f2607c3a",
   "metadata": {},
   "source": [
    "## 采集样本2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "28f43c1d",
   "metadata": {},
   "source": [
    "让夹爪尽量张大, 测量此时的夹爪间距，以及舵机原始位置。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "8e3534c7",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "alpha_1=54.8 夹爪张开距离: 68.0 mm\n"
     ]
    }
   ],
   "source": [
    "# 夹爪张开距离, 尺子测量得到\n",
    "width_1 = 68.0\n",
    "alpha_1 = gripper.get_servo_position()\n",
    "print(f\"alpha_1={alpha_1} 夹爪张开距离: {width_1} mm\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cb18c9e1",
   "metadata": {},
   "source": [
    "## 计算标定系数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "a3e64004",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "k_joint2servo: 49.7614\n",
      "b_joint2servo: 7.4522\n"
     ]
    }
   ],
   "source": [
    "result = gripper.calib_gripper_joint(alpha_close, width_1, alpha_1)\n",
    "k_joint2servo, b_joint2servo = result\n",
    "\n",
    "print(f\"k_joint2servo: {k_joint2servo:.4f}\")\n",
    "print(f\"b_joint2servo: {b_joint2servo:.4f}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bb149a03",
   "metadata": {},
   "source": [
    "修改`config/gripper_2f.yaml`里面的配置\n",
    "\n",
    "```yaml\n",
    "# 夹爪关节(rad)转换为夹爪舵机角度(deg)\n",
    "k_joint2servo: 57.8658\n",
    "b_joint2servo: 9.3310\n",
    "\n",
    "```"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
